//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include <core.h>
#include <_hal.h>

#if defined(_GNUC)

EXTERN_C DECL_CODEINIT void CDECL SetupCPU()
{
    // Make sure in svc mode
    ASM("mov    r0, %0;"
        "msr    cpsr_c, r0;"
        : :"i"(PSR_I | PSR_F | PSR_MODE_SVC) :"r0");

#ifdef _wmmx
    ASM("ldr r0, =0x0043;"
        "mcr p15, 0, r0, c15, c1, 0;"
        //CPWAIT
        "mrc p15, 0, r0, c2, c0, 0;"   // arbitrary read of CP15
        "mov r0, r0;"                  // wait for it
        "sub pc, pc, #4;"              // branch to next instruction
        // At this point, any previous CP15 writes are
        // guaranteed to have taken effect.

        // Clear wCon
        "mov r12, #3;"
        "mcr p1, 0, r12, c1, c0, 0;" // tmcr    wCon, r12
        );
#endif
}

#endif // defined(_GNUC)
